package edu.ortn.wildbots.commands;

import edu.ortn.wildbots.commands.*;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.ortn.wildbots.RobotMap;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;

public class Imaging extends CommandBase {
    
    ColorImage img;
    BinaryImage bimg;
    
    static final int HUE_MIN = 75;
    static final int HUE_MAX = 125;
    static final int SATURATION_MIN = 90;
    static final int SATURATION_MAX = 255;
    static final int LUM_MIN = 110;
    static final int LUM_MAX = 200;
       
    AxisCamera camera;
    
    CriteriaCollection cc;
    CameraControl camCont;
    
    DriverStationLCD lcd = DriverStationLCD.getInstance();
    
    public Imaging(CameraControl cc) {                       
       camCont = cc;
    }

    
    protected void initialize() {
        camera = AxisCamera.getInstance(RobotMap.CAM_IP);
        camera.writeResolution(AxisCamera.ResolutionT.k320x240);
        camera.writeMaxFPS(30);
        
        
        cc = new CriteriaCollection();
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400,false);
    }
    
    protected void execute() {      
        
        //System.out.println("Imaging execute() called");
    
        BinaryImage thresholdImage = null;
        BinaryImage bigObjectsImage = null;
        BinaryImage convexHullImage = null;
        BinaryImage filteredImage = null;
        try {
            if (camera.freshImage()) {
                img = camera.getImage();
                if (img == null)
                    return;
                
                thresholdImage = img.thresholdHSL(HUE_MIN, HUE_MAX,
                        SATURATION_MIN, SATURATION_MAX, LUM_MIN, LUM_MAX);
                bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);
                convexHullImage = bigObjectsImage.convexHull(false);
                filteredImage = convexHullImage.particleFilter(cc);           
     
                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();
                if (reports.length > 0) {
                    ParticleAnalysisReport r = reports[0];
                    lcd.println(DriverStationLCD.Line.kMain6, 1, 
                            "Particle: " + 1);
                    lcd.println(DriverStationLCD.Line.kUser2, 1, 
                    "COM x: " + r.center_mass_x);
                    
                    lcd.println(DriverStationLCD.Line.kUser3, 1, getDistance(r)+"");
                    particleTrack(r);
                }
                if (reports.length == 0) {
                    lcd.println(DriverStationLCD.Line.kUser2, 1, 
                            "COM x: Not found ");
                }
                lcd.println(DriverStationLCD.Line.kUser6, 1, 
                        filteredImage.getNumberParticles() + "  " + 
                        Timer.getFPGATimestamp());           
                lcd.updateLCD();                            
                        
            }
        } catch(Exception e) {
            e.printStackTrace();
        }
        finally {
            try {
                if (filteredImage != null)
                    filteredImage.free();
                if (convexHullImage != null)
                    convexHullImage.free();
                if (bigObjectsImage != null)
                     bigObjectsImage.free();
                if (thresholdImage != null)
                    thresholdImage.free();
                if (img != null)
                    img.free();
            } catch (NIVisionException ex) {
                ex.printStackTrace();
            }
        }
    }
    public double getDistance(ParticleAnalysisReport report){   //NOT DONE YET!
        double P1 = report.boundingRectWidth;
        //lcd.println(DriverStationLCD.Line.kUser4, 1, "Width: " + P1);
        return (320/(P1*(Math.tan(Math.toRadians(23.5)))));      
    }
    public boolean particleTrack(ParticleAnalysisReport r){
        double x = r.center_mass_x_normalized;
        double y = r.center_mass_y_normalized;
        if(x < -0.1){
            camCont.moveLeft();
        }
        if(x > 0.1){
            camCont.moveRight();
        }
        if(y < -0.1){
            camCont.moveUp();
        }
        if(y > 0.1){
            camCont.moveDown();
        }
        if(x > -0.1 && x < 0.1 && y > -0.1 && y < 0.1){
            return true;
        }else{
            return false;
        }
       
        
    }

   
    protected boolean isFinished() {
        return false;
    }

    
    protected void end() {
    }

    
    protected void interrupted() {
    }
}
    